#include <openrave-core.h>
#include <vector>
#include <sstream>
#include <boost/format.hpp>
#include <iostream>

#include <openrave/planningutils.h>

using namespace OpenRAVE;
using namespace std;

class BoxConfiguration : public boost::enable_shared_from_this<BoxConfiguration>
{
private:
	RobotBase::ManipulatorPtr _pmanip;
	KinBodyPtr _pbox, _ptarget;
	RobotBasePtr _probot;

public:
	BoxConfiguration(RobotBase::ManipulatorPtr pmanip, KinBodyPtr pbox)
	{
		_pmanip = pmanip;
		_pbox = pbox;
		_probot = pmanip->GetRobot();
		_ptarget = _pbox;
	}

	virtual ~BoxConfiguration() {
	}

	int GetDOF() 
	{
		return (int)_pmanip->GetArmIndices().size()+1;
	}

	void GetState(std::vector<dReal>& v)
	{
		_probot->GetActiveDOFValues(v);
		v.resize(GetDOF());
		v.back() = _pbox->GetTransform().trans.x;
	}

	bool _SetState(std::vector<dReal>& v, int filteroptions=IKFO_CheckEnvCollisions|IKFO_IgnoreCustomFilters) 
	{
		// save state before modifying it
		RobotBase::RobotStateSaverPtr savestate1(new RobotBase::RobotStateSaver(_probot));
		KinBody::KinBodyStateSaver savestate2(_ptarget);

		vector<dReal> vbox(1); 
		vbox[0] = v.back();

		_probot->SetActiveDOFValues(v);

		vector<dReal> vsolution;
		bool bsuccess = _pmanip->FindIKSolution(_ptarget->GetTransform(), vsolution, filteroptions);
		if( bsuccess ) {
			savestate1.reset();
			savestate2.Restore();
			_probot->SetActiveDOFValues(vsolution);
			std::copy(vsolution.begin(),vsolution.end(),v.begin());
		}
		return bsuccess;
	}

	void SetState(const std::vector<dReal>& v)
	{
		vector<dReal> vtemp = v;
		if( !_SetState(vtemp) ) {
			throw OPENRAVE_EXCEPTION_FORMAT0("could not set state",ORE_InvalidArguments);
		}
	}

	void SetPlannerParameters(PlannerBase::PlannerParametersPtr params)
	{
		_probot->SetActiveDOFs(_pmanip->GetArmIndices());
		params->_setstatefn = boost::bind(&BoxConfiguration::SetState,shared_from_this(),_1);
		params->_getstatefn = boost::bind(&BoxConfiguration::GetState,shared_from_this(),_1);
		params->SetRobotActiveJoints(_probot);
		params->_nMaxIterations = 4000; // max iterations before failure
	}
};